#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include "matrices.h"
#include "kalman.h"
#include "real_y_ruido.h"



/* Prueba 1 con Ts=0.1, M=400, F=200, lambda=0.6*/
/* 150 iteraciones */
/* Valores de sistema discretizado sacados de Matlab */
void prueba_1(){
	
	int contador=0;
	int reset_P;
	int random_n;
	str_matrix valores_ideal[150];
	str_matrix valores_ruido[150];
	str_matrix valores_kalman[150];
	str_matrix x1; str_matrix x2; str_matrix x3;
	
	double v;
	double w;
	
	double ruido_proceso=0.05;
	double ruido_medicion=10;
	
	double valores_Ex[4]={0.00,0.00,0.00,0.00};
	double valores_aux[4]={0.00,0.00,0.00,0.00};
	double valores_Ez[1]={100};
	double valores_Ad[4]={1,0.0999,0,0.9988};
	double valores_Bd[2]={0.0001,0.002};
	double valores_Cd[2]={1,0};
	double valores_Dd[1]={1};
	double valores_W[2]={1,1};
	
	str_matrix Ad;
	str_matrix Bd;
	str_matrix Cd;
	str_matrix Dd;
	str_matrix Ex;
	str_matrix Ez;
	str_matrix P;
	str_matrix W;
	
	str_kalman kalman;
	str_ideal ideal;
	str_ruido ruido;
	
	Ad=create_matrix(2,2);	Bd=create_matrix(2,1);
	Cd=create_matrix(1,2);	Dd=create_matrix(1,1);
	Ex=create_matrix(2,2); 	Ez=create_matrix(1,1);
	P=create_matrix(2,2);	W=create_matrix(2,1);
	
	set_values(&Ad,valores_Ad);	set_values(&Dd,valores_Dd);
	set_values(&Bd,valores_Bd);	set_values(&Ex,valores_Ex);
	set_values(&Cd,valores_Cd); set_values(&Ez,valores_Ez);
	set_values(&P,valores_Ex); set_values(&W,valores_W);
	
	kalman=create_kalman(&Ad,&Bd,&Cd,&Dd,&P,0.1,&Ex,&Ez);
	ideal=create_ideal(&Ad,&Bd,&Cd,0.1);
	ruido=create_ruido(&Ad,&Bd,&Cd,&Dd,&W,0.1);
	
	srand(time(NULL));
	printf("PRUEBA 1\n");
	printf("Ts=0.1\t M=400\t F=200\t lambda=0.6\t 150 iteraciones\n ");
	printf("ruido proceso=0.05\t ruido medicion=10\n");
	printf("X_ideal\t\t X_ruido \t X_kalman \n");
	
	reset_P=0;
	
	while(contador<150){
		random_n=rand() % 2001;
		v=ruido_medicion*(((double)random_n)/1000.00 - 1.00);
		random_n=rand() % 2001;
		w=ruido_proceso*(((double)random_n)/1000.00 - 1.00);
		
		/*printf("%f \n",v);
		printf("%f \n",w);*/
		
		/*if(reset_P>=5){
			restore_P(&kalman,valores_aux);
			reset_P=0;
		}
		*/
		x1=ideal_step(&ideal,200);
		x2=ruido_step(&ideal,&ruido,200,w,v);
		x3=kalman_step(&kalman,200,&x2);
		
		valores_ideal[contador]=x1;
		valores_ruido[contador]=x2;
		valores_kalman[contador]=x3;
		
		/*mostrar_matrix_2(&x1);mostrar_matrix_2(&x2);mostrar_matrix_2(&x3);*/
		printf("%f\t",*(x1.data+1));printf("%f\t",*(x2.data+1));printf("%f\t",*(x3.data+1));
		printf("\n");
		
		destroy_matrix(&x1);
		destroy_matrix(&x2);
		destroy_matrix(&x3);
		
		contador++;
	}
	
}

/* Prueba 2 con Ts=0.1, M=400, F=200, lambda=0.6*/
/* 20 iteraciones */
/* Valores de sistema discretizado sacados de Matlab */
void prueba_2(){

	int contador=0;
	int random_n;
	str_matrix valores_ideal[20];
	str_matrix valores_ruido[20];
	str_matrix valores_kalman[20];
	str_matrix x1; str_matrix x2; str_matrix x3;

	double v;
	double w;
	
	double ruido_proceso=0.05;
	double ruido_medicion=10;

	double valores_Ex[4]={0.0000013,0.000025,0.000025,0.0005};
	double valores_Ez[1]={100};
	double valores_Ad[4]={1,0.1,0,0.9997};
	double valores_Bd[2]={0.000025,0.0004999};
	double valores_Cd[2]={1,0};
	double valores_Dd[1]={1};
	double valores_W[2]={1,1};
	
	str_matrix Ad;
	str_matrix Bd;
	str_matrix Cd;
	str_matrix Dd;
	str_matrix Ex;
	str_matrix Ez;
	str_matrix P;
	str_matrix W;
	
	str_kalman kalman;
	str_ideal ideal;
	str_ruido ruido;
	
	Ad=create_matrix(2,2);	Bd=create_matrix(2,1);
	Cd=create_matrix(1,2);	Dd=create_matrix(1,1);
	Ex=create_matrix(2,2); 	Ez=create_matrix(1,1);
	P=create_matrix(2,2);	W=create_matrix(2,1);
	
	set_values(&Ad,valores_Ad);	set_values(&Dd,valores_Dd);
	set_values(&Bd,valores_Bd);	set_values(&Ex,valores_Ex);
	set_values(&Cd,valores_Cd); set_values(&Ez,valores_Ez);
	set_values(&P,valores_Ex); set_values(&W,valores_W);
	
	kalman=create_kalman(&Ad,&Bd,&Cd,&Dd,&P,0.1,&Ex,&Ez);
	ideal=create_ideal(&Ad,&Bd,&Cd,0.1);
	ruido=create_ruido(&Ad,&Bd,&Cd,&Dd,&W,0.1);

	srand(time(NULL));
	printf("PRUEBA 2\n");
	printf("Ts=0.1\t M=400\t F=200\t lambda=0.6\t 20 iteraciones\n ");
	printf("ruido proceso=0.05\t ruido medicion=10\n");
	printf("X_ideal\t\t X_ruido \t X_kalman \n");
	
	while(contador<20){
		random_n=rand() % 2001;
		v=ruido_medicion*(((double)random_n)/1000.00 - 1.00);
		random_n=rand() % 2001;
		w=ruido_proceso*(((double)random_n)/1000.00 - 1.00);

		/*printf("%f \n",v);
		printf("%f \n",w);*/
		
		x1=ideal_step(&ideal,200);
		x2=ruido_step(&ideal,&ruido,200,w,v);
		x3=kalman_step(&kalman,200,&x2);
		
		valores_ideal[contador]=x1;
		valores_ruido[contador]=x2;
		valores_kalman[contador]=x3;

		/*mostrar_matrix_2(&x1);mostrar_matrix_2(&x2);mostrar_matrix_2(&x3);*/
		printf("%f\t",*(x1.data+1));printf("%f\t",*(x2.data+1));printf("%f\t",*(x3.data+1));
		printf("\n");
		
		destroy_matrix(&x1);
		destroy_matrix(&x2);
		destroy_matrix(&x3);
		
		contador++;
	}
	
}
/* Prueba 3 con Ts=0.1, M=400, F=200, lambda=0.6*/
/* 100 iteraciones */
/* Valores de sistema discretizado sacados de Matlab */
void prueba_3(){
	
	int contador=0;
	int reset_P;
	int random_n;
	str_matrix valores_ideal[100];
	str_matrix valores_ruido[100];
	str_matrix valores_kalman[100];
	str_matrix x1; str_matrix x2; str_matrix x3;
	
	double v;
	double w;
	
	double ruido_proceso=0.05;
	double ruido_medicion=10;
	
	double valores_Ex[4]={0.0000013,0.000025,0.000025,0.0005};
	double valores_Ez[1]={100};
	double valores_Ad[4]={1,0.1,0,0.9997};
	double valores_Bd[2]={0.000025,0.0004999};
	double valores_Cd[2]={1,0};
	double valores_Dd[1]={1};
	double valores_W[2]={1,1};
	
	str_matrix Ad;
	str_matrix Bd;
	str_matrix Cd;
	str_matrix Dd;
	str_matrix Ex;
	str_matrix Ez;
	str_matrix P;
	str_matrix W;
	
	str_kalman kalman;
	str_ideal ideal;
	str_ruido ruido;
	
	Ad=create_matrix(2,2);	Bd=create_matrix(2,1);
	Cd=create_matrix(1,2);	Dd=create_matrix(1,1);
	Ex=create_matrix(2,2); 	Ez=create_matrix(1,1);
	P=create_matrix(2,2);	W=create_matrix(2,1);
	
	set_values(&Ad,valores_Ad);	set_values(&Dd,valores_Dd);
	set_values(&Bd,valores_Bd);	set_values(&Ex,valores_Ex);
	set_values(&Cd,valores_Cd); set_values(&Ez,valores_Ez);
	set_values(&P,valores_Ex); set_values(&W,valores_W);
	
	kalman=create_kalman(&Ad,&Bd,&Cd,&Dd,&P,0.1,&Ex,&Ez);
	ideal=create_ideal(&Ad,&Bd,&Cd,0.1);
	ruido=create_ruido(&Ad,&Bd,&Cd,&Dd,&W,0.1);
	
	srand(time(NULL));
	printf("PRUEBA 3\n");
	printf("Ts=0.1\t M=400\t F=200\t lambda=0.6\t 100 iteraciones\n ");
	printf("ruido proceso=0.05\t ruido medicion=10\n");
	printf("X_ideal\t\t X_ruido \t X_kalman \n");
	reset_P=0;
	while(contador<100){
		random_n=rand() % 2001;
		v=ruido_medicion*(((double)random_n)/1000.00 - 1.00);
		random_n=rand() % 2001;
		w=ruido_proceso*(((double)random_n)/1000.00 - 1.00);
		
		/*printf("%f \n",v);
		printf("%f \n",w);*/
		
		x1=ideal_step(&ideal,200);
		x2=ruido_step(&ideal,&ruido,200,w,v);
		x3=kalman_step(&kalman,200,&x2);
		
		valores_ideal[contador]=x1;
		valores_ruido[contador]=x2;
		valores_kalman[contador]=x3;
		
		/*mostrar_matrix_2(&x1);mostrar_matrix_2(&x2);mostrar_matrix_2(&x3);*/
		printf("%f\t",*(x1.data+1));printf("%f\t",*(x2.data+1));printf("%f\t",*(x3.data+1));
		printf("\n");
		
		destroy_matrix(&x1);
		destroy_matrix(&x2);
		destroy_matrix(&x3);
		
		contador++;
	}
	
}



int main()
{

	
	prueba_1();
	
	/*
	prueba_2();
	*/
	/*
	prueba_3();
	*/
	
	return 0;

}
